#include "PointLabTools/Kdtree/Kdtree.h"

namespace PointlabTools
{
	KDTree::KDTree(PointLabCore::IPointCloud* cloud) : FlannKDTree(3 /*dim*/, *(new PointCloudAdaptor(cloud)), KDTreeSingleIndexAdaptorParams(30 /* max leaf */))
	{
		FlannKDTree::buildIndex();
	}

	KDTree::~KDTree()
	{
	}

	void KDTree::knnSearch(const PointLabCore::Vector3& point, const PointLabCore::Uint k, std::vector<PointLabCore::Uint>& indices, std::vector<PointLabCore::Real>& distance)
	{
		FlannKDTree::knnSearch(&point.x, k, &indices[0], &distance[0]);
	}

	PointLabCore::Uint KDTree::radiusSearch(const PointLabCore::Vector3& point, const PointLabCore::Real &radius, std::vector<PointLabCore::Uint>& indices, std::vector<PointLabCore::Real>& distance, bool sort)
	{
		std::vector<std::pair<PointLabCore::Uint, PointLabCore::Real> > IndicesDists;
		SearchParams searchParams;
		searchParams.sorted = sort;
		const PointLabCore::Uint npts = FlannKDTree::radiusSearch(&point.x, radius, IndicesDists, searchParams);

		indices.resize(npts);
		distance.resize(npts);

		for (PointLabCore::Uint i = 0; i < npts; i++)
		{
			indices[i] = IndicesDists[i].first;
			distance[i] = IndicesDists[i].second;
		}
		return npts;
	}
}